//Application
#include "step.h"
#include "robot_cmd.h"
#include "robot_def.h"
//modules
#include "message_center.h"
#include "step_motor.h"
//modules
#include "general_def.h"
#include "bsp_dwt.h"
#include "arm_math.h"
#include "remote_control.h"
#include "gpio.h"
#include "tim.h"

//控制yaw 和 pitch的步进电机的声明

StepInstance *yaw_step, *pitch_step;

static Publisher_t *stand_pub_x;                    // 用于发布飞镖架的数据
static Subscriber_t *stand_sub_x;                   // 用于订阅飞镖架的控制命令

static Publisher_t *stand_pub_y;                    // 用于发布飞镖架的数据
static Subscriber_t *stand_sub_y;                   // 用于订阅飞镖架的控制命令

static Stand_Ctrl_Cmd_s_x stand_cmd_recv_x;         // 飞镖架接收到的控制命令_x
static Stand_Upload_Data_s stand_feedback_data_x; // 飞镖架回传的反馈数据_x

static Stand_Ctrl_Cmd_s_y stand_cmd_recv_y;         // 飞镖架接收到的控制命令_y
static Stand_Upload_Data_s stand_feedback_data_y; // 飞镖架回传的反馈数据_y
static  double v; //步进电机的移动速度

static uint8_t switch_state = 0;

/* 飞镖架初始化函数 */

void StepTaskInit()
{   
    Step_Init_Config_s step_conf;
    step_conf.htim = &htim8;
    step_conf.Channel = TIM_CHANNEL_2;
    step_conf.DIR.GPIOx = GPIOB ;
    step_conf.DIR.GPIO_Pin = GPIO_PIN_14;
    step_conf.ENA.GPIOx = GPIOF;
    step_conf.ENA.GPIO_Pin = GPIO_PIN_0;
    step_conf.Init_frequency = 100000;
    step_conf.Init_perangle = 0.225;
    step_conf.mode = 2;
    step_conf.sareturn.pulse_count = 0;
    step_conf.sareturn.pulse_ccr = 500;
    step_conf.dir_flag = 1;

    yaw_step = StepInit(&step_conf);

    step_conf.htim = &htim8;
    step_conf.Channel = TIM_CHANNEL_3;
    step_conf.DIR.GPIOx = GPIOB ;
    step_conf.DIR.GPIO_Pin = GPIO_PIN_15;
    step_conf.ENA.GPIOx = GPIOF;
    step_conf.ENA.GPIO_Pin = GPIO_PIN_1;
    step_conf.Init_frequency = 100000;
    step_conf.Init_perangle = 0.225;
    step_conf.mode = 2;
    step_conf.sareturn.pulse_count = 0;
    step_conf.sareturn.pulse_ccr = 500;
    step_conf.dir_flag = 1;

    pitch_step = StepInit(&step_conf);

    //注意yaw 和 pitch 的消息要分开，不然会出现堆栈弹出的冲突，导致步进电机出现规律性卡顿
    stand_sub_x = SubRegister("stand_cmd_x", sizeof(Stand_Ctrl_Cmd_s_x));
    stand_pub_x = PubRegister("stand_feed_x", sizeof(Stand_Upload_Data_s));


    stand_sub_y = SubRegister("stand_cmd_y", sizeof(Stand_Ctrl_Cmd_s_y));
    stand_pub_y = PubRegister("stand_feed_y", sizeof(Stand_Upload_Data_s));
}

//以下分开声明的操作同理
float x_angle = 0;
float y_angle = 0;

void StepTask_x()
{
    SubGetMessage(stand_sub_x, &stand_cmd_recv_x);
    v = stand_cmd_recv_x.v;
    if (stand_cmd_recv_x.dx > 50.0f)
    {
        x_angle++;
    }
    else if (stand_cmd_recv_x.dx < -50.0f)
    {
        x_angle--;
    }
    else
    {
        x_angle=x_angle;
    }

    Set_abs_SA(yaw_step,x_angle,v);

    PubPushMessage(stand_pub_x, (void *)&stand_feedback_data_x);
}

void StepTask_y()
{

    SubGetMessage(stand_sub_y, &stand_cmd_recv_y);
    
    if (stand_cmd_recv_y.dy > 50.0f)
    {
        y_angle++;
    }
    else if (stand_cmd_recv_y.dy < -50.0f)
    {
        y_angle--;
    }
    else
    {
        y_angle = y_angle;
    }

    Set_abs_SA(pitch_step,y_angle,180);

    PubPushMessage(stand_pub_y, (void *)&stand_feedback_data_y);
}

//切换击打目标为前哨站
void Step_outpost(){
    x_angle = x_angle-1601-150;
    Set_abs_SA(yaw_step,x_angle,144.0f);
    // Set_abs_SA(pitch_step,y_angle,180);
}

//切换击打目标为基地
void Step_base(){
    x_angle = x_angle+1601+150;
    Set_abs_SA(pitch_step,y_angle,144.0f);
    // Set_abs_SA(yaw_step,x_angle,180);

}

void switch_state_task(){
    switch_state = HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_14);
    if (switch_state == 1){
        x_angle = x_angle;
        Set_abs_SA(pitch_step,y_angle,720);
        HAL_GPIO_WritePin(GPIOF,GPIO_PIN_0,SET);
        HAL_GPIO_WritePin(GPIOH,GPIO_PIN_12,SET);
    }
}







